#include "Arm.h"

//Public functions

//Constructors
Arm::Arm(){}

//Initializers
void Arm::initArm()
{
	_tool_loc = (_tcp_ptr) malloc( sizeof( _tcp ) );

	unsigned int tempLimits[ _NUM_AXIS ][4] = {
		{ 750, 2250, 1500, 750 },
		{ 750, 2250, 1500, 1500 },
		{ 600, 2100, 1500, 1500 },
		{ 600, 2400, 1500, 1500 },
		{ 600, 2100, 1500, 1500 },
		{ 1350, 2400, 1500, 2400 }}; //Not sure on gripper yet
	memcpy( _ARM_LIMITS, tempLimits, sizeof( _ARM_LIMITS ) );
	this->gripperClose();
	_gripper_open = 0;  //start with gripper closed.	


	Servo temp[ _NUM_AXIS ];
	//Should probably make the magic 2 some constant pin offset or something.
	for( int i = 0; _NUM_AXIS > i; i++ )
	{
		_axis_array[ i ] = (_axis_ptr) malloc( sizeof( _axis ) );
	}
	for( int i = 0; _NUM_AXIS > i; i++ )
	{	
		 //Not sure why the new keyword isn't working here.	
		//_axis_array[ i ]->servo = new Servo;
		_axis_array[ i ]->servo = &temp[ i ];
		_axis_array[ i ]->servo->attach( ( i + 2 ), /* Pins start at 2 */ 
						_ARM_LIMITS[ i ][ 0 ],
						_ARM_LIMITS[ i ][ 1 ] );
		_axis_array[ i ]->min = _ARM_LIMITS[ i ][ 0 ]; 
		_axis_array[ i ]->max = _ARM_LIMITS[ i ][ 1 ]; 
		_axis_array[ i ]->neutral = _ARM_LIMITS[ i ][ 2 ]; 
		_axis_array[ i ]->calibPos = _ARM_LIMITS[ i ][ 3 ]; 
	}

}

//Accessors
void Arm::getTCP( double tcp[ 3 ] )
{
	tcp[ 0 ] = this->getTCPX();
	tcp[ 1 ] = this->getTCPY();
	tcp[ 2 ] = this->getTCPZ();
}

double Arm::getTCPX()
{
	return _tool_loc->tcp_x;
}

double Arm::getTCPY()
{
	return _tool_loc->tcp_y;
}

double Arm::getTCPZ()
{
	return _tool_loc->tcp_z;
}

//Mutators
void Arm::setTCP( double x, double y, double z, double grip_angle )
{
	
}

void Arm::goHome()
{
	//This position reduces current draw by 50% from calibrate position.
	this->setAxisAngle( 2, 135 );
	this->setAxisAngle( 3, 45 );
}

void Arm::goToCal()
{
	for( int i = 0; i < _NUM_AXIS; i++)
	{
		_axis_array[ i ]->servo->writeMicroseconds( 
					_axis_array[ i ]->calibPos );
	}
}

int Arm::setAxisAngle( int axisNum, int angle )
{
	//This needs to be updated to use the look-up table.
	angle = angle*10+600; //convert to microseconds
	axisNum = axisNum--; //Convert to array indicies
	int errorCode = 0;

	//Do not allow gripper to be adjusted this way.
	if( axisNum >= 0 && axisNum <= 5)
	{
		//Prevent exceeding mechcanical limits.
		if( _axis_array[ axisNum ]->min > angle )
		{
			_axis_array[ axisNum ]->servo->
					writeMicroseconds( 
					_axis_array[ axisNum ]->min );
			errorCode = -1;
		}
		else if( _axis_array[ axisNum ]->max < angle )
		{	
			_axis_array[ axisNum ]->servo->
					writeMicroseconds( 
					_axis_array[ axisNum ]->max );
			errorCode = -2;
		}
		else
		{
			_axis_array[ axisNum ]->servo->
					writeMicroseconds( angle );
			errorCode = 0;
		}
	}
	return errorCode;
}

void Arm::gripperOpen()
{
	if( !_gripper_open )
	{
		_axis_array[ 5 ]->servo->
			writeMicroseconds( _axis_array[ 5 ]->min );
		_gripper_open = 1;
	}
}

void Arm::gripperClose()
{
	if( _gripper_open )
	{
		_axis_array[ 5 ]->servo->
			writeMicroseconds( _axis_array[ 5 ]->max );
		_gripper_open = 0;
	}
}
